import sensor, image, time
from pyb import UART
import json
import ustruct
THRESHOLD = (40, 15, 61, 17, 55, -39)
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()
uart = UART(3,115200)
uart.init(115200, bits=8, parity=None, stop=1)
ROI =[(0,35,20,25),
	  (60,35,20,25),
	  (30,0,25,25)]
while(True):
	clock.tick()
	left = right =shizi=tleft=turn=0
	img = sensor.snapshot().binary([THRESHOLD])
	line = img.get_regression([(100,100)], robust = True)
	for rec in ROI:
		img.draw_rectangle(rec, color=(255,0,0))
	if (line):
		rho_err = abs(line.rho())-img.width()/2
		if line.theta()>90:
			theta_err = line.theta()-180
		else:
			theta_err = line.theta()
		img.draw_line(line.line(), color = 127)
		if(rho_err<0):
		   flag = 0
		   rho_err = -rho_err
		else:
		   flag = 2
		   rho_err =rho_err
		if img.find_blobs([(96, 100, -13, 5, -11, 18)],roi=ROI[0]):
		   right=1
		if img.find_blobs([(96, 100, -13, 5, -11, 18)],roi=ROI[1]):
		   left=1
		if img.find_blobs([(96, 100, -13, 5, -11, 18)],roi=ROI[2]):
		   tleft=1
		if right==1 and left==1:
		   shizi =1
		if tleft==1 and left==1:
		   turn =1
		if right==1 and tleft==1:
		   turn =2
		print(shizi)
		print(turn)
		x=int(rho_err)
		y=int(line.magnitude())
		data = bytearray([0xb3,x,y,flag,shizi,turn,tleft,0x5b])
		uart.write(data)
		time.sleep_ms(10)
uart.deinit()